from ros2webserver.baseserver.capability import Capability
import rclpy

class Nav2Pose(Capability):

    def __init__(self, protocol):
        Capability.__init__(self, protocol)
        protocol.register_operation("goal", self.goal)
        protocol.register_operation("cancle", self.cancle)

    def goal(self, message):
        self.protocol.node_handle.get_logger().info(f"{message}")
        status = self.protocol.nav_handle.nav_status
        self.protocol.nav_handle.get_logger().info(f"nav {status}")
        data = message['data']
        poses = data['poses']
        if status!='active' or status!='inactive' and len(poses)>0:
            if len(poses)==1:
                pose = poses[0]
                target_pose = self.protocol.nav_handle.get_pose_by_xyyaw(pose['xPose'],pose['yPose'],pose['angle'])
                result = self.protocol.nav_handle.go_to_pose(target_pose)
                self.protocol.nav_handle.get_logger().info(f"nav to target_pose: {target_pose} ,result:{result}")
            else:
                posw_array = []
                for pose in poses:
                    target_pose = self.protocol.nav_handle.get_pose_by_xyyaw(pose['xPose'],pose['yPose'],pose['angle'])
                    posw_array.append(target_pose)
                result = self.protocol.nav_handle.go_to_poses(posw_array)
                self.protocol.nav_handle.get_logger().info(f"nav to {posw_array} ,result:{result}")
            if result:
                self.protocol.send({
                    "op":"goal",
                    "code":0,
                    "msg":'ok',  
                    "data":None
                })
                return
        self.protocol.send({
            "op":"goal",
            "code":1,
            "msg":'failed',  
            "data":None
        })
    def cancle(self,message):
        self.protocol.node_handle.get_logger().info(f"{message}")
        result = self.protocol.nav_handle.cancle();
        self.protocol.send({
            "op":"cancle",
            "code":0,
            "msg":'ok',  
            "data":None
        })

    def finish(self):
        pass
